﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Drawing.Drawing2D;
using ExtensionMethods;
using System.Collections;
using MLRobotic.IA.Drivers;
using System.IO.Ports;
using System.Threading;
using MLRobotic.IA;
using MLRobotic.Asserv;


namespace MLRobotic.Simulateur
{
    public partial class frm_SimuBack : Form
    {
        //private static SerialPort com18;
        public static COM UART;
        public static bool Complugged = true;
        public static List<string> dataToLog = new List<string>();

        public static int scale = 2;
        public static float rotation = 0;

        DateTime endTime = new DateTime();

        private Rectangle rec_background;
        private Rectangle rec_brdBot;
        private Rectangle rec_brdTop;
        private Rectangle rec_brdLeft;
        private Rectangle rec_brdRight;
        private Rectangle rec_DepartBleu;
        private Rectangle rec_DepartRouge;
        private Rectangle rec_bufLeftBot;
        private Rectangle rec_bufLeftTop;
        private Rectangle rec_bufRightTop;
        private Rectangle rec_bufRightBot;
        private Rectangle rec_Gateau;

        private bool[] ls_couleurBougie = new bool[16];
        private List<Rectangle> ls_bougie = new List<Rectangle>();

        public static Rectangle rec_Adversaire;

        public static Rectangle robot;
        Rectangle printedRobot;
        Image imgEcran = new Bitmap(1550, 1050);
        public List<Rectangle> ls_obstacles = new List<Rectangle>();
        System.Threading.Timer tmr_CalculPosition;

        public bool CollisionRobot = false;


        static Brush blackbrush = new SolidBrush(Color.Black);
        static Brush whitebrush = new SolidBrush(Color.White);
        static Brush redbrush = new SolidBrush(Color.Red);
        static Brush bluebrush = new SolidBrush(Color.Blue);
        static Pen whitepen = new Pen(whitebrush, 8);
        Image img_backIni = Image.FromFile("fond_placeRobot.png");
        Image img_back = Image.FromFile("fond_placeRobot.png");
        Rectangle lastPaintedRobot = new Rectangle(0, 0, 0, 0);

        public frm_SimuBack()
        {
            endTime = DateTime.Now.AddSeconds(90);

            DefinirObstacles();
          
            ls_bougie.Add(RectCercleInscrit(40, 1074, 82));
            ls_bougie.Add(RectCercleInscrit(40, 1104, 194));
            ls_bougie.Add(RectCercleInscrit(40, 1164, 297));
            ls_bougie.Add(RectCercleInscrit(40, 1248,379));
            ls_bougie.Add(RectCercleInscrit(40,1351,438));
            ls_bougie.Add(RectCercleInscrit(40, 1463,470));
            ls_bougie.Add(RectCercleInscrit(40, 1180,90));
            ls_bougie.Add(RectCercleInscrit(40, 1228,219));
            ls_bougie.Add( RectCercleInscrit(40,1328,316));
            ls_bougie.Add( RectCercleInscrit(40,1454,367));

            //this.rec_Gateau = new Rectangle(new Point(610, 11), new Size(300, 300));
            this.rec_Gateau = RectCercleInscrit(500, 1522, 22);
            ls_obstacles.Add(rec_Gateau);
            robot = new Rectangle(new Point(23, 125), new Size(210, 305));

            Deplace.Init();

            Deplace.Xposmm = robot.X+robot.Width/2;
            Deplace.Yposmm = robot.Y + robot.Height / 2;
            Deplace.TetaPos = 0;


            ChangeBougiesColor();
            InitializeComponent();

            bt_connectCOM_Click(null, null);
            tmr_SendAdversaire = new System.Threading.Timer(new TimerCallback(tmr_SendAdversaire_tick),null, Timeout.Infinite, 500);
            tmr_CalculPosition = new System.Threading.Timer(new System.Threading.TimerCallback(tmr_calculposition_Tick),null,25,25);

        }

        void DefinirObstacles()
        {
            this.rec_background = new Rectangle(new Point(422, 22), new Size(2200, 2000));
            this.rec_brdTop = new Rectangle(new Point(-400, -400), new Size(3844, 422));
            ls_obstacles.Add(rec_brdTop);
            this.rec_brdBot = new Rectangle(new Point(-400, 2022), new Size(3844, 422));
            ls_obstacles.Add(rec_brdBot);
            this.rec_brdLeft = new Rectangle(new Point(-400, -400), new Size(422, 3844));
            ls_obstacles.Add(rec_brdLeft);
            this.rec_brdRight = new Rectangle(new Point(3022, -400), new Size(422, 2844));
            ls_obstacles.Add(rec_brdRight);
            this.rec_DepartBleu = new Rectangle(new Point(22, 22), new Size(400, 2000));
            this.rec_DepartRouge = new Rectangle(new Point(2622, 22), new Size(400, 2000));

            this.rec_bufLeftTop = new Rectangle(new Point(22, 22), new Size(400, 100));
            ls_obstacles.Add(rec_bufLeftTop);
            this.rec_bufLeftBot = new Rectangle(new Point(22, 1922), new Size(400, 100));
            ls_obstacles.Add(rec_bufLeftBot);
            this.rec_bufRightTop = new Rectangle(new Point(2622, 22), new Size(400, 100));
            ls_obstacles.Add(rec_bufRightTop);
            this.rec_bufRightBot = new Rectangle(new Point(2622, 1922), new Size(400, 100));
            ls_obstacles.Add(rec_bufRightBot);

            rec_Adversaire = new Rectangle(new Point(2760, 220), new Size(250, 150));

            ls_obstacles.Add(rec_Adversaire);
        }

        public Rectangle RectCercleInscrit(int r, int posX, int posY)
        {
            int rectx =  posX-r;
            int recty= posY -r;
            return new Rectangle(new Point(rectx, recty), new Size(2*r, 2*r));
        }
        
        public void DrawRotatedRectangle(Graphics g, Rectangle r, float angle)
        {
            using (Matrix m = new Matrix())
            {
                printedRobot = ScaleRectangle(r);
                m.RotateAt(angle, new PointF(printedRobot.Left + (printedRobot.Width / 2),
                                          printedRobot.Top + (printedRobot.Height / 2)));
                g.Transform = m;
                
                if (CollisionRobot)
                    g.FillRectangle(redbrush, printedRobot);
                else
                    g.FillRectangle(blackbrush, printedRobot);
                g.DrawLine(whitepen, new Point(printedRobot.Right,printedRobot.Top+(printedRobot.Height/2)),new Point((printedRobot.Right + 15),printedRobot.Top+(printedRobot.Height/2)));
                g.ResetTransform();  
            }
        }

        Rectangle ScaleRectangle(Rectangle r)
        {
            return new Rectangle(Convert.ToInt32(r.X * coef), Convert.ToInt32(r.Y * coef), Convert.ToInt32(r.Width * coef), Convert.ToInt32(r.Height * coef));
        }

        void tmr_CompteRebour_Tick(object sender, EventArgs e)
        {
            TimeSpan tp = endTime.Subtract(DateTime.Now);
            if (tp.TotalSeconds > 0)
                lb_time.Text = string.Format("{0:0}:{1:00}", tp.Minutes, tp.Seconds);
            else
                tmr_CompteRebour.Enabled = false;
        }

        private void frm_SimuBack_Resize(object sender, EventArgs e)
        {
            pbMain.Size = this.ClientSize;

            if (pbMain.Height > 0)
                coef = Math.Min(pbMain.Height * 1.0 / img_backIni.Height, pbMain.Width * 1.0 / img_backIni.Width);

            img_back = new Bitmap(Convert.ToInt32(img_backIni.Width * coef), Convert.ToInt32(img_backIni.Height * coef));
            Graphics g = Graphics.FromImage(img_back);

            g.DrawImage(img_backIni, 0, 0, img_back.Width, img_back.Height);
            imgEcran = new Bitmap(img_back.Width, img_back.Height);

            coef = Math.Min(pbMain.Height / 1022.0, pbMain.Width / 1522.0) / 2;

        }

        bool isDrawing = false;
        public void RedessinerImage()
        {
            if (lastPaintedRobot.Width == 0)
                pbMain.Invalidate();
            else
            {
                if (!isDrawing)
                {
                    isDrawing = true;
                    UpdateImage();
                    lastPaintedRobot = printedRobot;
                    // redessin de la zone
                    if (adversaireABouger)
                    {
                        pbMain.CreateGraphics().DrawImage(imgEcran, 0, 0);
                        adversaireABouger = false;
                    }
                    else
                    {
                        Rectangle rectToRedraw = GetFullRedrawZone(lastPaintedRobot, printedRobot);
                        pbMain.CreateGraphics().DrawImage(imgEcran, rectToRedraw, rectToRedraw, GraphicsUnit.Pixel);
                    }
                    isDrawing = false;
                }
            }
            //pbMain.Invalidate();

            lb_robotPos.Text = (int)Deplace.Xposmm + " ; " + (int)Deplace.Yposmm;
            lb_adversPos.Text = (rec_Adversaire.Location.X + rec_Adversaire.Width / 2) + " ; " + (rec_Adversaire.Location.Y + rec_Adversaire.Height / 2);
            lb_angle.Text = (Deplace.TetaPos * 180 / Math.PI % 360).ToString();
            if (frm_SimuBack.dataToLog.Count > 0)
            {
                tb_event.AppendText(frm_SimuBack.dataToLog[0] + "\n");
                frm_SimuBack.dataToLog.RemoveAt(0);
            }

        }


        /// <summary>
        /// Permet de connaitre la zone couvrant le robot de départ et d'arrivée
        /// pour invalidate et paint
        /// </summary>
        /// <param name="rectangle"></param>
        /// <param name="rectangle_2"></param>
        /// <returns></returns>
        private Rectangle GetFullRedrawZone(Rectangle rectangle, Rectangle rectangle_2)
        {
            return new Rectangle(Math.Min(rectangle.X, rectangle_2.X) - rectangle.Width,
                Math.Min(rectangle.Y, rectangle_2.Y) - rectangle.Height,
                Math.Max(Math.Abs(rectangle.Right - rectangle_2.Left), Math.Abs(rectangle.Left - rectangle_2.Right)) + rectangle.Width * 2,
                Math.Max(Math.Abs(rectangle.Top - rectangle_2.Bottom), Math.Abs(rectangle.Bottom - rectangle_2.Top)) + rectangle.Height * 2);
        }

        private void bt_go_Click(object sender, EventArgs e)
        {
            bt_go.Enabled = false;
            endTime = DateTime.Now.AddSeconds(90);
            tmr_CompteRebour.Start();

            tmr_SendAdversaire.Change(0, 2000);
            frm_SimuBack.UART.Send(MLRobotic.IA.Drivers.COM.typePacket.IamASimulateur);
            frm_SimuBack.UART.Send(MLRobotic.IA.Drivers.COM.typePacket.SimuTiretteDepartEnlevee);


            if (Complugged)
            {
                UART.Send(ls_couleurBougie.toUShort(), 0, IA.Drivers.COM.typePacket.RetourBougie);
                UART.Send(COM.typePacket.IamASimulateur);
            }
            else
                Log.Write("SIMU : " + IA.Drivers.COM.typePacket.RetourBougie.ToString());

        }

        //private void frm_SimuBack_KeyPress(object Ssender, KeyPressEventArgs e)
        //{
        //    switch (e.KeyChar) {
        //        case 'z':
        //            robot.Location = new Point(robot.X, robot.Y - 10);
        //            break;
        //        case 's':
        //            robot.Location = new Point(robot.X, robot.Y + 10);
        //            break;
        //        case 'q':
        //            robot.Location = new Point(robot.X - 10, robot.Y);
        //            break;
        //        case 'd':
        //            robot.Location = new Point(robot.X+ 10, robot.Y);
        //            break;
        //        default:
        //            Console.WriteLine("Typed char: "+e.KeyChar.ToString());
        //            break;
        //    }
        //    RedessinerImage();
        //}

        double coef;
        private void pbMain_Paint(object sender, PaintEventArgs e)
        {

            UpdateImage();
            e.Graphics.DrawImage(imgEcran, 0, 0);
        }

        private void UpdateImage()
        {
            Graphics g = Graphics.FromImage(imgEcran);
            g.DrawImage(img_back, 0, 0);
            g.FillRectangle(redbrush,ScaleRectangle(rec_Adversaire));
            DrawRotatedRectangle(g, robot, rotation);
            DrawBougies(g);
        }

        private void pbMain_MouseDown(object sender, MouseEventArgs e)
        {
            Point p = pbMain.PointToClient(e.Location);
            p.X = (int)(e.X / coef);
            p.Y = (int)(e.Y / coef);
            if (e.Button == System.Windows.Forms.MouseButtons.Left)
            {
                tb_event.AppendText("GOTOXY (" + p.X + "," + p.Y + ")\n");
                Deplace.GotoXY(p.X, p.Y, true);
            }
            else
            {
                rec_Adversaire = new Rectangle(p.X - rec_Adversaire.Width / 2, p.Y - rec_Adversaire.Height / 2, rec_Adversaire.Width, rec_Adversaire.Height);
                adversaireABouger = true;
            }

            
        }
        
        private void DrawBougies(Graphics g)
        {
            int i = 0;
            foreach (Rectangle r in ls_bougie)
            {
                if (ls_couleurBougie[i] == true)
                {
                    g.FillEllipse(bluebrush, ScaleRectangle(r));
                    
                    g.FillEllipse(redbrush, ScaleRectangle(MirroredRectangle(r)));
                }
                else
                {
                    g.FillEllipse(redbrush, ScaleRectangle(r));
                    g.FillEllipse(bluebrush, ScaleRectangle(MirroredRectangle(r)));
                }
                //Console.WriteLine(ScaleRectangle(r).ToString());
                //Console.WriteLine(ScaleRectangle(MirroredRectangle(r)).ToString());
                i++;
            }

        }

        private Rectangle MirroredRectangle(Rectangle rectangle)
        {
            return new Rectangle(new Point(2960 - rectangle.Left, rectangle.Top), rectangle.Size);
        }


        internal static void DoRotate(double p)
        {
            Deplace.TetaPos += p;
            rotation = (int)(Deplace.TetaPos*180/Math.PI);        
        }

        internal static void DoAvance(int p)
        {
            robot.X = robot.X + (int)Math.Round(Math.Cos(Deplace.TetaPos), 0);
            robot.Y = robot.Y + (int)Math.Round(Math.Sin(Deplace.TetaPos), 0);
        }

        private void timer1_Tick(object sender, EventArgs e)
        {

            RedessinerImage();
        }

        private void pbMain_MouseMove(object sender, MouseEventArgs e)
        {
            Point p = pbMain.PointToClient(e.Location);
            p.X = (int)(e.X / coef);
            p.Y = (int)(e.Y / coef);
            lb_curspos.Text="(" + p.X + "," + p.Y + ")";
        }


        static DateTime lastCalculPosition = DateTime.Now;
        double tempspasse;
        private void tmr_calculposition_Tick(object sender)
        {
            lock (collisionMap)
            {
                tempspasse = DateTime.Now.Subtract(lastCalculPosition).TotalMilliseconds;
                lastCalculPosition = DateTime.Now;
                Deplace.DeplaceBouge();
                Deplace.CalculPosition();
                //Deplace.Move(tempspasse);
                robot.X = (int)(Deplace.Xposmm - robot.Width / 2);
                robot.Y = (int)(Deplace.Yposmm - robot.Height / 2);
                rotation = (int)(Deplace.TetaPos * 180 / Math.PI);
                CollisionRobot = (RobotEnCollision(robot, Deplace.TetaPos));
            }
        }
        private void tmr_Dessin_Tick(object sender)
        {
            lock (collisionMap)
            {
                tempspasse = DateTime.Now.Subtract(lastCalculPosition).TotalMilliseconds;
                lastCalculPosition = DateTime.Now;
                Deplace.DeplaceBouge();
                Deplace.CalculPosition();
                //Deplace.Move(tempspasse);
                robot.X = (int)(Deplace.Xposmm - robot.Width / 2);
                robot.Y = (int)(Deplace.Yposmm - robot.Height / 2);
                rotation = (int)(Deplace.TetaPos * 180 / Math.PI);
                CollisionRobot = (RobotEnCollision(robot, Deplace.TetaPos));
            }
        }
        

        Image collisionMap = MLRobotic.Simulateur._2013.Properties.Resources.fond;
        Rectangle? totalSize = null;
        private bool adversaireABouger = false;

        private bool RobotEnCollision(Rectangle robot, double teta)
        {
            if(totalSize == null)
                totalSize = new Rectangle(0, 0, collisionMap.Width, collisionMap.Height);
            lock (collisionMap)
            {
                foreach (Point sommetRobot in GetSommetRobot(robot, teta))
                {
                    if (!totalSize.Value.Contains(sommetRobot) || ((Bitmap)collisionMap).GetPixel(sommetRobot.X, sommetRobot.Y).GetBrightness() < 0.5)
                        return true;
                    else if (rec_Adversaire.Contains(sommetRobot.X,sommetRobot.Y))
                        return true;
                    //foreach (Rectangle obstacle in ls_obstacles)
                    //{
                    //    if (obstacle.Contains(sommetRobot))
                    //        return true;
                    //}
                }
            }
            return false;
        }

        List<Point> GetSommetRobot(Rectangle robot, double teta)
        {
           List<Point> ls_point = new List<Point>();

           for (double co = -0.5; co <= 0.5; co += 1.0 / 16)
           {
               ls_point.Add(RotatePoint(robot, teta, -robot.Width / 2, robot.Height * co));
               ls_point.Add(RotatePoint(robot, teta, robot.Width / 2, robot.Height * co));
               ls_point.Add(RotatePoint(robot, teta, robot.Width * co, -robot.Height / 2));
               ls_point.Add(RotatePoint(robot, teta, robot.Width * co, robot.Height / 2));
           }

            return ls_point;
        }



        private static Point RotatePoint(Rectangle robot, double teta, double xRobot, double yRobot)
        {
            return new Point(
                          (int)(xRobot * Math.Cos(teta) - yRobot * Math.Sin(teta) + robot.X + robot.Width / 2),
                          (int)(xRobot * Math.Sin(teta) + yRobot * Math.Cos(teta) + robot.Y + robot.Height / 2));
        }

        private void rb_moveRobot_CheckedChanged(object sender, EventArgs e)
        {
            rd_moveAdversaire.Checked = !this.rb_moveRobot.Checked;
        }

        private void rd_moveAdversaire_CheckedChanged(object sender, EventArgs e)
        {
            rb_moveRobot.Checked = !this.rd_moveAdversaire.Checked;
        }

        private void ChangeBougiesColor()
        {
            int nb_red = 0;
            int nb_blue = 0;
            Random rnd = new Random();
            for (int i = 1; i <10; i++)
            {
                ls_couleurBougie[0] = true;
                ls_couleurBougie[6] = true;
                if (nb_red != 5)
                {
                    if (nb_blue != 4)
                    {
                        if (i != 6)
                            ls_couleurBougie[i] = (rnd.Next(0, 2) == 0);
                        if (ls_couleurBougie[i] == true)
                            nb_blue++;
                        else
                            nb_red++;
                    }
                    else
                    {
                        ls_couleurBougie[i] = false;
                        nb_red++;
                    }
                }
                else
                {
                    ls_couleurBougie[i] = true;
                    nb_blue++;
                }
                
            }
        }

        private void button1_Click(object sender, EventArgs e)
        {
            ChangeBougiesColor();

            if (Complugged)
                UART.Send(ls_couleurBougie.toUShort(),0, IA.Drivers.COM.typePacket.RetourBougie);
            else
                Log.Write("SIMU : " + IA.Drivers.COM.typePacket.RetourBougie.ToString());
                
            this.Invalidate(true);
        }

        public void AppendToTbEvent(string s)
        {
            tb_event.AppendText(s);
        }

        static System.Threading.Timer tmr_GetUart;
        public static System.Threading.Timer tmr_SendAdversaire;

        static void tmr_GetUart_Tick(object sender)
        {
            if (Complugged)
                UART.Process();
        }

        static void tmr_SendAdversaire_tick(object sender)
        {
            if (Complugged)
            {
                //Log.Write("SIM : Send Position Adverse");
                UART.Send((short)rec_Adversaire.Center().X, (short)rec_Adversaire.Center().Y, COM.typePacket.RetourPositionAdversaire);

            }
        }

        private void bt_demandePosition_Click(object sender, EventArgs e)
        {
            if (Complugged)
                UART.Send(COM.typePacket.DemandePosition);
            else
                Log.Write("SIM : Demande Postion");
        }

        private void bt_connectCOM_Click(object sender, EventArgs e)
        {
            if ((UART== null) || (!UART.isOpen))
                try
                {
                    UART = new COM("COM5");
                    tmr_GetUart = new System.Threading.Timer(new TimerCallback(tmr_GetUart_Tick), null, 200,50);
                    bt_connectCOM.Text = "COM connected";
                    bt_connectCOM.BackColor = Color.Green;
                    Complugged = true;

                }
                catch (Exception ex)
                {
                    Log.Write("Error loading UART : " + ex.Message);
                    Complugged = false;
                }
        }

        private void frm_SimuBack_FormClosing(object sender, FormClosingEventArgs e)
        {
            try
            {
                Complugged = false;
                UART.Dispose();

            }
            catch (Exception ex)
            {
                Log.Write("Unable to close the COM port"+ex.Message);
            }
        }

        #region Action des bouttons
        private void bt_stop_Click(object sender, EventArgs e)
        {
            Deplace.Stop();
        }

        private void bt_GotoXy_Click(object sender, EventArgs e)
        {
           
        }

        private void tb_x_Enter(object sender, EventArgs e)
        {
            tb_x.SelectionStart = 0;
            tb_x.SelectionLength = tb_x.Text.Length;
        }


        private void tb_y_Enter(object sender, EventArgs e)
        {
            tb_y.SelectionStart = 0;
            tb_y.SelectionLength = tb_y.Text.Length;
        }

        private void tb_teta_Enter(object sender, EventArgs e)
        {
            tb_teta.SelectionStart = 0;
            tb_teta.SelectionLength = tb_teta.Text.Length;
        }

        private void tb_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyData == Keys.Enter)
            {
                try
                {
                        Deplace.GotoXY(int.Parse(tb_x.Text), int.Parse(tb_y.Text), cb_avant.Checked);
                        Log.Write("KEY : GOTOXY (" + int.Parse(tb_x.Text) + "," + int.Parse(tb_y.Text) + ")");
                   
                }
                catch (Exception ex)
                {
                    Log.Write("SIM : not a number : "+ex.Message);
                }
            }
        }

        private void tb_teta_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyData == Keys.Enter)
            {
                try
                {
                    if (cb_radianDegre.Checked)
                        Deplace.GotoTeta(Double.Parse(tb_teta.Text));
                    else
                        Deplace.GotoTeta(Double.Parse(tb_teta.Text) * System.Math.PI / (double)180);
                }
                catch (Exception ex)
                {
                    Log.Write("SIM : not a number :" + ex.Message);
                }
            }
            else if ((e.KeyData == Keys.Decimal) || (e.KeyData == Keys.Oemcomma) || (e.KeyData== Keys.OemPeriod))
            {
                tb_teta.Text += ",";
                e.SuppressKeyPress = true;
                tb_teta.Select(tb_teta.Text.Length, 0);
            }
        }

        private void cb_radianDegre_CheckedChanged(object sender, EventArgs e)
        {
            if (!cb_radianDegre.Checked)
            {
                cb_radianDegre.Text = "Degré";
                try
                {
                    tb_teta.Text.Replace('.', ',');
                    tb_teta.Text = (Double.Parse(tb_teta.Text) * System.Math.PI / (double)180).ToString().Substring(0,4);
                }
                catch (Exception ex)
                {
                    Log.Write("SIM : not a number :"+ ex.Message);
                }
            }
            else
            {
                cb_radianDegre.Text = "Radian";
                try
                {
                    tb_teta.Text = (Double.Parse(tb_teta.Text) / System.Math.PI * (double)180).ToString().Substring(0, 4);
                }
                catch (Exception ex)
                {
                    Log.Write("SIM : not a number :" + ex.Message);
                }
            }
        }
        #endregion

    }
}
